<?xml version="1.0" ?>
<sdf version="1.6">

  <model name="robotx_dock_2018">

    <!-- The 2018 dock base -->
    <include>
      <name>dock_2018_base</name>
      <uri>model://dock_2018_base_dynamic</uri>
    </include>
    <%
      # Constants
      dock_x                = 7.5
      dock_y                = 3
      dock_z                = 1.5
      dock_roll             = 0
      dock_pitch            = 0
      dock_yaw              = Math::PI / 2.0
      circle_offset_x       = 0.06
      circle_offset_y       = 0
      circle_offset_z       = 0.6
      circle_offset_roll    = 0
      circle_offset_pitch   = Math::PI / 2.0
      circle_offset_yaw     = -Math::PI / 2.0
      cross_offset_x        = 0.06
      cross_offset_y        = 0
      cross_offset_z        = 0.6
      cross_offset_roll     = 0
      cross_offset_pitch    = Math::PI / 2.0
      cross_offset_yaw      = -Math::PI / 2.0
      triangle_offset_x     = 0.06
      triangle_offset_y     = -0.5
      triangle_offset_z     = 0.2
      triangle_offset_roll  = Math::PI / 2.0
      triangle_offset_pitch = 0
      triangle_offset_yaw   = 0
      placard_offset_x      = -1
      placard_offset_y      = 0
      placard_offset_z      = 0
      placard_offset_roll   = 0
      placard_offset_pitch  = 0
      placard_offset_yaw    = 0

      # Variables
      placard_x     = dock_x
      placard_y     = dock_y
      placard_z     = dock_z + 0.1
      placard_roll  = dock_roll
      placard_pitch = dock_pitch
      placard_yaw   = dock_yaw

      circle_x     = dock_x     + circle_offset_x
      circle_y     = dock_y     + circle_offset_y
      circle_z     = dock_z     + circle_offset_z
      circle_roll  = dock_roll  + circle_offset_roll
      circle_pitch = dock_pitch + circle_offset_pitch
      circle_yaw   = dock_yaw   + circle_offset_yaw

      cross_x     = dock_x     + cross_offset_x
      cross_y     = dock_y     + cross_offset_y
      cross_z     = dock_z     + cross_offset_z
      cross_roll  = dock_roll  + cross_offset_roll
      cross_pitch = dock_pitch + cross_offset_pitch
      cross_yaw   = dock_yaw   + cross_offset_yaw

      triangle_x     = dock_x     + triangle_offset_x
      triangle_y     = dock_y     + triangle_offset_y
      triangle_z     = dock_z     + triangle_offset_z
      triangle_roll  = dock_roll  + triangle_offset_roll
      triangle_pitch = dock_pitch + triangle_offset_pitch
      triangle_yaw   = dock_yaw   + triangle_offset_yaw
    %>
    <% for i in 1..2 %>
    <!-- Placard #<%= i %> -->
    <model name="dock_2018_placard<%= i %>">
      <pose><%=placard_x%> <%=placard_y%> <%=placard_z%> <%=placard_roll%> <%=placard_pitch%> <%=placard_yaw%></pose>
      <include>
        <pose>0 0 0.25 0 0 0</pose>
        <uri>model://placard</uri>
      </include>
      <link name="link_symbols">
        <% if i == 1%>
        <pose> 0 -0.1 0.75 0 0 0 </pose>
        <% else %>
        <pose> 0 0.1 0.75 0 0 0 </pose>
        <% end %>
        <visual name="visual_circle">
          <pose>0 0 0 1.571 0 0</pose>
          <geometry>
            <cylinder>
              <radius>0.5</radius>
              <length>0.001</length>
            </cylinder>
          </geometry>
          <transparency>1</transparency>
          <plugin name="placard<%= i %>_plugin" filename="libplacard_plugin.so">
            <!-- Uncomment for setting the shape and color. -->
            <!--
            <shape>cross</shape>
            <color>blue</color>
            -->
            <visuals>
              <visual>robotx_dock_2018::dock_2018_placard<%= i %>::link_symbols::visual_circle</visual>
              <visual>robotx_dock_2018::dock_2018_placard<%= i %>::link_symbols::visual_h_cross</visual>
              <visual>robotx_dock_2018::dock_2018_placard<%= i %>::link_symbols::visual_v_cross</visual>
              <visual>robotx_dock_2018::dock_2018_placard<%= i %>::link_symbols::visual_triangle</visual>
            </visuals>
            <shuffle>true</shuffle>
            <robot_namespace>vrx/dock_2018_placard<%= i %></robot_namespace>
            <ros_shuffle_topic>shuffle</ros_shuffle_topic>
          </plugin>
        </visual>
        <visual name="visual_h_cross">
          <pose>0 0 0 1.571 0 0</pose>
          <geometry>
            <box>
              <size>0.99 0.33 0.001</size>
            </box>
          </geometry>
          <transparency>1</transparency>
        </visual>
        <visual name="visual_v_cross">
          <pose>0 0 0 1.571 0 0</pose>
          <geometry>
            <box>
              <size>0.33 0.99 0.001</size>
            </box>
          </geometry>
          <transparency>1</transparency>
        </visual>
        <visual name="visual_triangle">
          <pose> -0.5 0 -0.5 1.571 0 0</pose>
          <geometry>
            <mesh><uri>model://symbol_triangle/mesh/triangle.dae</uri></mesh>
          </geometry>
          <transparency>1</transparency>
        </visual>
      </link>

      <joint name="placard_to_visual" type="fixed">
        <pose> 0 0 0 0 0 0 </pose>
        <parent>placard::link</parent>
        <child>link_symbols</child>
      </joint>
      
      <!-- For visualizing ContainPlugin volume 
           Make sure that the size and location are consistent with plugin -->
      <link name="bay_visualization">
        <visual name="bay_box">
          <% if i == 1%>
          <pose frame="placard::link">0 -5 -1.5 0 0 0</pose>
          <% else %>
          <pose frame="placard::link">0  5 -1.5 0 0 0</pose>
          <% end %>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Green</name>
            </script>
          </material>
          <geometry>
            <box>
              <size>1.5 4 2</size>
            </box>
          </geometry>
          <transparency>0.0</transparency>
        </visual>
        <visual name="bay_box_exterior">
          <% if i == 1%>
          <pose frame="placard::link">0 -10 -1.5 0 0 0</pose>
          <% else %>
          <pose frame="placard::link">0  10 -1.5 0 0 0</pose>
          <% end %>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Red</name>
            </script>
          </material>
          <geometry>
            <box>
              <size>8 1.5 2</size>
            </box>
          </geometry>
          <transparency>0.0</transparency>
        </visual>
      </link>
      <joint name="placard_to_bay" type="fixed">
        <child>placard::link</child>
        <parent>bay_visualization</parent>
      </joint>

    </model>
    <%
      placard_x     += placard_offset_x
      placard_y     += placard_offset_y
      placard_z     += placard_offset_z
      placard_roll  += placard_offset_roll
      placard_pitch += placard_offset_pitch
      placard_yaw   += placard_offset_yaw

      circle_x     += (placard_offset_x  - 2 * circle_offset_x)
      circle_y     += placard_offset_y
      circle_z     += placard_offset_z
      circle_roll  += placard_offset_roll
      circle_pitch += placard_offset_pitch
      circle_yaw   += placard_offset_yaw

      cross_x     += (placard_offset_x  - 2 * cross_offset_x)
      cross_y     += placard_offset_y
      cross_z     += placard_offset_z
      cross_roll  += placard_offset_roll
      cross_pitch += placard_offset_pitch
      cross_yaw   += placard_offset_yaw

      triangle_x     += (placard_offset_x - 2 * triangle_offset_x)
      triangle_y     += placard_offset_y
      triangle_z     += placard_offset_z
      triangle_roll  += placard_offset_roll
      triangle_pitch += placard_offset_pitch
      triangle_yaw   += placard_offset_yaw
    %>
    <% end %>
  
    <joint name = "joint_placard1" type="fixed">
      <pose>0 0 3 0 0 0 </pose>
      <parent>dock_2018_placard1::placard::link</parent>
      <child>dock_2018_base::dock_block_4_1::link</child>
    </joint>

    <joint name = "joint_placard2" type="fixed">
      <pose>0 0 3 0 0 0 </pose>
      <parent>dock_2018_placard2::placard::link</parent>
      <child>dock_2018_base::dock_block_3_1::link</child>
    </joint>
  </model>
</sdf>
